/* 
 * GripBall Project.
 * 
 * Copyright 2006 POB-Technology. All rights reserved.
 * POB-Technology PROPRIETARY/CONFIDENTIAL.
 * 
 */
#include <pob-eye.h>


#include "grip_pattern.h" //Pattern file
#include "grip_bitmap.h"  //Bitmap file

#include "grip.h" //Servomotor grip order


//function for pob-proto and move bot
static void InitPobProto (void);
static void MoveAndStop(UInt8 Way,UInt32 time);
static void MoveBot(UInt8 Way);

//move order for pob-proto board
#define MOVE_RUN		0x05
#define MOVE_BACK		0x0A
#define MOVE_LEFT		0x09
#define MOVE_RIGHT		0x06
#define STOP_BOT		0


//array for display real time video
static UInt8         LCD_Fast_Buffer[64*64*BYTES];
static GraphicBuffer LCD_Fast_Buffer_Video;

//array for display the graphic interface
static GraphicBuffer LCD_Buffer_Video;
static UInt8         LCD_Buffer[64*64*BITS];

//function for displat real time video
static void DrawVision(RGBFrame *ptr);


//main program
int main(void)
{
	int i ;
	RGBFrame rgbFrame ;
	HSLFrame hslFrame ;

	int exitHouseLoop = 0 ;
	UInt8 NbForm ;
	Form ListOfForm[MAX_OF_FORM];

	int BallX = 0 ;
	int BallY = 0 ;

	InitPOBEYE();

	InitLCD();

	InitPobProto();
	SwitchOnAllServo();

	//init graphic buffer
	InitGraphicBuffer(&LCD_Buffer_Video,64,64,ONE_BIT,LCD_Buffer); 

	InitGraphicBuffer(&LCD_Fast_Buffer_Video,64,64,EIGHT_BITS,LCD_Fast_Buffer);
	ClearGraphicBuffer(&LCD_Fast_Buffer_Video);

	//init frame pointer
	GetPointerOnRGBFrame(&rgbFrame);

	GetPointerOnHSLFrame(&hslFrame);

	//init head
	HeadInit();
	HeadUp();

	//init grip
	GripMiddle();
	GripOpen();


	//set head to see ball
	HeadDownToBall();

	ClearGraphicBuffer(&LCD_Buffer_Video);
	DrawBitmap(5,20,IDB_SEARCHBALL2,grip_bitmap,&LCD_Buffer_Video);
	DrawRightLCD(&LCD_Buffer_Video);

	//red ball loop
	while(1)
	{
		//grab hsl frame
		GrabRGBFrame();
		GetHSLFrame(&hslFrame);

		//hsl frame and rgb frame share the same memory aera
		//so we can change value of hsl frame for display it on lcd screen
		for(i=0 ; i < HEIGHT_FRAME*WIDTH_FRAME ; i++ )
		{
			if( hslFrame.hue[i] == 0 )
				hslFrame.hue[i] = 1 ;
			else
				hslFrame.hue[i] = 0 ;
		}

		DrawVision(&rgbFrame);

		if( FoundBall( &hslFrame, &BallX,&BallY) == 0 )
		{
		//	PrintTextOnPobTerminal("Found ball at X %d Y %d", BallX,BallY);

			if( BallY < 50 )
			{
				MoveAndStop(MOVE_RUN,20000);
			}
			
			if( BallX > 35 )
			{
				MoveAndStop(MOVE_RIGHT,20000);
			}
			else if( BallX < 25 )
			{
				MoveAndStop(MOVE_LEFT,20000);
			}
			
			
			if( BallX > 25 && BallX < 35 && BallY > 50 )
			{
				GripDown();
				Wait(600000);
				GripCatchBall();
				Wait(600000);
				HeadUp();
				GripUp();
				break ;
			}
		}
	}

	ClearGraphicBuffer(&LCD_Buffer_Video);
	DrawBitmap(5,20,IDB_SEARCHFORM,grip_bitmap,&LCD_Buffer_Video);
	DrawRightLCD(&LCD_Buffer_Video);

	//house loop
	while( exitHouseLoop == 0 )
	{
		GrabRGBFrame();
		BinaryRGBFrame(&rgbFrame);

		//draw camera frame
		DrawVision(&rgbFrame);


		NbForm = IdentifyForm(&rgbFrame,ListOfForm,grip_pattern);								

		if( NbForm == 0 )
		{
			MoveAndStop(MOVE_LEFT,20000);
		}
		else
		{
			exitHouseLoop = GoToHouse( ListOfForm, NbForm);
		}
	}

	Wait(400000);
	GripMiddle();
	GripClose();
	Wait(10000);
	HeadInit();
	Wait(10000);
	MoveAndStop(MOVE_BACK,800000);
	

	return 0 ;
}


/** Go to the house function.
 *
 * @param formArray : pointer to a list of form in the frame
 * @param nbForm : form's number
 *
 * @return -1 if bot is just front of the house, 0 else.
 *
 */
int GoToHouse( Form *formArray, int nbForm)
{
	int i ;

	int MoveOrder = STOP_BOT ;

	int HeadPosition = 139 ;

	for(i=0 ; i<nbForm ; i++ )
	{
		if( formArray[i].id == IDP_HOUSE )
		{
			//move head to center the form
			if( formArray[i].y>65)
			{
				HeadPosition = HeadPosition - 4;
			}
			else if( formArray[i].y<55)
			{
				HeadPosition = HeadPosition + 4;
			}

			SetServoMotor(HEAD_SERVO,HeadPosition);

			//move bot to center the form
			if( formArray[i].x > 55 )
			{
				MoveAndStop(MOVE_RIGHT,20000);
			}
			else if( formArray[i].x < 35 )
			{
				MoveAndStop(MOVE_LEFT,20000);
			}
			else //go to house with the bot
			{

				//PrintTextOnPobTerminal("form at w=%d h=%d", ListOfForm[i].width ,ListOfForm[i].height );

				
				if( formArray[i].width > 20 && formArray[i].height > 40 &&
					formArray[i].width < 25 && formArray[i].height < 45)
				{
					//go slow to the house
					MoveBot(STOP_BOT);
					MoveAndStop(MOVE_RUN,20000);
				}
				else if( formArray[i].width > 25 && formArray[i].height > 45)
				{
					//stop move and open grip
					MoveBot(STOP_BOT);
					GripUp();
					Wait(200000);
					GripOpen();
					return -1 ;
					
				}
				else
				{
					//go fast to the house
					MoveOrder=MOVE_RUN ;
				}
			
			}
			
		}
	}

	MoveBot(MoveOrder);

	return 0 ;
}


/** Found ball function.
 *
 * @param hsl : pointer on a valid hsl frame
 * @param foundX : x ball coordinate
 * @param foundY : y ball coordinate
 *
 * @return 0 if ball found, -1 if failed to found a ball
 *
 */
int FoundBall(HSLFrame *hsl, int *foundX, int *foundY)
{
	int i,j ;

	int result = -1 ;
	int p1,p2,p3,p4 ;
	int s1,s2,s3,s4 ;

	//saturation parameters :
	int saturationLevel = 199 ;
	int saturationAvg = 199 ;
	//coordinate for search ball
	int WidthRectangle = 23 ;
	int HeightRectangle = 38 ;

	int saturationMax = 0 ;
			
	for(i=0 ; i < WIDTH_FRAME-WidthRectangle ; i++ )
	{
		for(j=0 ; j < HEIGHT_FRAME-HeightRectangle ; j++ )
		{
			p1 = i+j*WIDTH_FRAME;
			p2 = i+WidthRectangle+j*WIDTH_FRAME;
			p3 = i+(j+HeightRectangle)*WIDTH_FRAME;
			p4 = i+WidthRectangle+(j+HeightRectangle)*WIDTH_FRAME;
			
			//if hue component is egual to 0, get saturation component and verify level and average
			if( hsl->hue[p1] == 0 && 
				hsl->hue[p2] == 0 &&
				hsl->hue[p2] == 0 &&
				hsl->hue[p3] == 0 )
			{
				s1 = hsl->saturation[p1];
				s2 = hsl->saturation[p2];
				s3 = hsl->saturation[p3];
				s4 = hsl->saturation[p4];
				
				int saturationAverage = (s1+s2+s3+s4)/4 ;
				
				if( s1>saturationLevel && s2>saturationLevel && s3>saturationLevel && s4>saturationLevel
					&& saturationAverage>=saturationAvg )
				{
					
					if( saturationAverage > saturationMax )
					{	
						saturationMax = saturationAverage ;
						*foundX = i ;
						*foundY = j ;
						result = 0 ;
					}
					
				}
				
			}
			
		}
		
	}

	return result;
}

/**
 * Initialize the POB-PROTO board. PORTA set as analogic, PORTC as servomotor.
 */
void InitPobProto (void)
{
	PobProto board;
	

	//to get the position of the analogic joystick, you have to set the PORTA as analogic input
	board.porta=ALL_PORTA_AS_ANA;	

	//all pin of PORTC are configured to manage servomotors
	board.portc=RC7_AS_SERVO |RC6_AS_SERVO |RC3_AS_SERVO |RC2_AS_SERVO |RC1_AS_SERVO |RC0_AS_SERVO;
	

	//RD0 RD1 RD2 RD3 are configured as digitals output to gear DC motor
	//RD4 RD5 RD6 RD7 are configured as digitals input
	board.portd=RD7_AS_DI |RD6_AS_DI |RD5_AS_DI |RD4_AS_DI |RD3_AS_DO |RD2_AS_DO |RD1_AS_DO |RD0_AS_DO;
	
	//set the pob proto
	SetPobProto(&board);

}

/**
 * Move and stop bot.
 *
 * @param Way : move order
 * @param time : time to move
 */
void MoveAndStop( UInt8 Way, UInt32 time )
{
	MoveBot(Way);
	Wait(time);
	MoveBot(STOP_BOT);
}

/**
 * Move the bot
 *
 * @param Way : move bot order
 */
void MoveBot (UInt8 Way)
{	
	SetPortD(Way);
}

/** 
 * Draw the frame binarised 
 *
 * @param ptr : pointer to a valid RGBFrame
 */
void DrawVision (RGBFrame *ptr)
{	
	//draw the red buffer (88*120 pixels) on the Buffer for for the left screen (64*64 pixels)
	DrawComponentInABufferVideo(ptr->red,&LCD_Fast_Buffer_Video);

	DrawLeftLCD(&LCD_Fast_Buffer_Video); //refresh the left screen
}
